import rclpy                        # ROS2 Python接口库
from rclpy.node import Node         # ROS2 节点类
from sensor_msgs.msg import Image   # 图像消息类型
from cv_bridge import CvBridge      # ROS与OpenCV图像转换类
import cv2                          # Opencv图像处理库

running_time = 10
OUTPUT_DIR = "output/"

class ImageCap(Node):
    def __init__(self, name):
        super().__init__(name)
        
        self.pub = self.create_publisher(Image,"image_capture",3)
        self.timer = self.create_timer(0.1, self.capture_callback)
        self.capture = cv2.VideoCapture(0)
        self.transport = CvBridge()
        self.cnt = 0

    def capture_callback(self):
        ret, frame = self.capture.read()
        imgmsg = self.transport.cv2_to_imgmsg(frame, 'bgr8')
        if ret == True:
            self.pub.publish(imgmsg)
        self.get_logger().info(f"Publishing Image Message {self.cnt}")
        cv2.imwrite(f"{OUTPUT_DIR}input_{self.cnt}.jpg",frame)
        self.cnt += 1

    def over_load(self):
        global running_time
        if running_time <= 0:
            return True
        if self.cnt >= running_time:
            return True
        return False

def main(arg=None):
    rclpy.init(args=arg)
    node = ImageCap("topic_image_pub")
    while not node.over_load():
        rclpy.spin_once(node)
    node.destroy_node()
    rclpy.shutdown()
